#include <os/msgpool.h>
#include <os/schedule.h>
#include <os/task.h>
#include <lib/string.h>

msgpool_t *MsgpoolCreate(size_t msgsize, size_t msgcount)
{
    msgpool_t *pool;

    if (!msgsize || !msgcount)
        return NULL;
        
    pool = KMemAlloc(sizeof(msgpool_t));
    if (!pool)
        return NULL;
    pool->msgsize = msgsize;
    pool->msgcount = 0;
    pool->msgmax = msgcount;
    pool->msgbuff = KMemAlloc(msgsize * msgcount);
    if (!pool->msgbuff)
        return NULL;
    memset(pool->msgbuff, 0, msgsize * msgcount);
    MutexlockInit(&pool->mutex);
    WaitQueueInit(&pool->waiter);
    pool->head = pool->tail = pool->msgbuff;
    return pool;
}

int MsgpoolDestroy(msgpool_t *pool)
{
    if (!pool)
        return -1;
    if (WaitQueueLen(&pool->waiter) > 0)
    {
        WaitQueueWakeupAll(&pool->waiter);
    }
    MutexlockLock(&pool->mutex, MUTEX_LOCK_MODE_BLOCK);
    pool->msgcount = 0;
    pool->msgmax = 0;
    pool->msgsize = 0;
    KMemFree(pool->msgbuff);
    pool->tail = pool->head = pool->msgbuff = NULL;
    MutexlockUnlock(&pool->mutex);
    KMemFree(pool);
    return 0;
}

int MsgpoolPut(msgpool_t *pool, void *buff, size_t size)
{
    if (!pool || !size || !buff)
        return -1;
    MutexlockLock(&pool->mutex, MUTEX_LOCK_MODE_BLOCK);
    if (MsgpoolFull(pool))
    {
        WaitQueueAdd(&pool->waiter, cur_task);
        MutexlockUnlock(&pool->mutex);
        TaskBlock(TASK_BLOCKED);
        MutexlockLock(&pool->mutex, MUTEX_LOCK_MODE_BLOCK);
    }
    memcpy(pool->head, buff, size);
    pool->head += pool->msgsize;
    if(pool->head==pool->msgbuff+pool->msgmax*pool->msgsize)
        pool->head=pool->msgbuff;
    pool->msgcount++;    
    MutexlockUnlock(&pool->mutex);

    if (WaitQueueLen(&pool->waiter) > 0)
    {
        WaitQueueWakeup(&pool->waiter);
    }
    return 0;
}

int MsgpoolTryPut(msgpool_t *pool, void *buff, size_t size)
{
    if (!pool || !size || !buff)
        return -1;
    MutexlockLock(&pool->mutex, MUTEX_LOCK_MODE_BLOCK);
    if (MsgpoolFull(pool))
    {
        MutexlockUnlock(&pool->mutex);
        return -1;
    }
    memcpy(pool->head, buff, min(pool->msgsize, size));
    pool->head += pool->msgsize;
    if(pool->head==pool->msgbuff+pool->msgmax*pool->msgsize)
        pool->head=pool->msgbuff; 
    pool->msgcount++;
    MutexlockUnlock(&pool->mutex);

    if (WaitQueueLen(&pool->waiter) > 0)
    {
        WaitQueueWakeup(&pool->waiter);
    }
    return 0;
}

int MsgpoolGet(msgpool_t *pool, void *buff, msgpool_func_t callback)
{
    if (!pool)
        return -1;
    MutexlockLock(&pool->mutex, MUTEX_LOCK_MODE_BLOCK);
    if (MsgpoolEmpty(pool))
    {
        WaitQueueAdd(&pool->waiter, cur_task);
        MutexlockUnlock(&pool->mutex);
        TaskBlock(TASK_BLOCKED);
        MutexlockLock(&pool->mutex, MUTEX_LOCK_MODE_BLOCK);
    }
    if (buff) //buffer is present
    {

        if (callback)
        {
            callback(pool, buff);
        }
        else
        {
            memcpy(buff, pool->tail, pool->msgsize);
        }
    }
    pool->tail += pool->msgsize;
    if(pool->tail==pool->msgbuff+pool->msgmax*pool->msgsize)
        pool->tail=pool->msgbuff; 
    pool->msgcount--;
    MutexlockUnlock(&pool->mutex);

    if (WaitQueueLen(&pool->waiter) > 0)
    {
        WaitQueueWakeup(&pool->waiter);
    }
    return 0;
}

int MsgpoolTryGet(msgpool_t *pool, void *buff, msgpool_func_t callback)
{
    if (!pool)
        return -1;
    MutexlockLock(&pool->mutex, MUTEX_LOCK_MODE_BLOCK);
    if (MsgpoolEmpty(pool))
    {
        MutexlockUnlock(&pool->mutex);
        return -1; 
    }
    if (buff) //buffer is present
    {
        if (callback)
        {
            callback(pool, buff);
        }
        else
        {
            memcpy(buff, pool->tail, pool->msgsize);
        }
    }
    pool->tail += pool->msgsize;    
    if(pool->tail==pool->msgbuff+pool->msgmax*pool->msgsize)
        pool->tail=pool->msgbuff;
    pool->msgcount--;
    MutexlockUnlock(&pool->mutex);

    if (WaitQueueLen(&pool->waiter) > 0)
    {
        WaitQueueWakeup(&pool->waiter);
    }
    
    return 0;
}
